/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.assets.shooterstates;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.templates.assets.statemachine.State;
import edu.wpi.first.wpilibj.templates.assets.statemachine.StateMachine;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
 *
 * @author Jarrod Risley
 */
public class AutonShoot extends State {

    /*
     * Global Class Variables
     */
    protected static AutonShoot SHOOT_STATE =
            new AutonShoot("Shoot State");
    
    private double startTime;
    private boolean shooterRampedUp = false;
    
    private static final double TIMEOUT_TIME = 1;
    
    private int timesRun = 0;
    
    // CONSTRUCTOR //
    
    
    private AutonShoot(String name) {
        super(name);
    }  // end ShootState Constructor
    
    protected void runFirst() {
        
        
        setNext(ZingStateMachine.AU_SPIN_UP);
        setTimeoutTimer(TIMEOUT_TIME);
        timesRun++;
        
    } // end runFirst

    protected void run() {
        
        startTime = Timer.getFPGATimestamp();
        
        do {
            
            CommandBase.shooter.indexerShoot();
            
        } while (startTime + 0.5 > Timer.getFPGATimestamp());
        
        CommandBase.shooter.indexerStop();
        
        setFinished();
        
    } // end run

    protected void runLast() {
        
        if (timesRun >= 3) {
            
            CommandBase.shooter.stop();
            setNext(null);
            
        } // end if
        
    } // end runLast

    protected boolean isFinished() {
        return getFinished();
    } // end isFinished

    protected void end() {
        
    } // end end...lolz =P

    protected State getChainedInstance(State nextState) {
        
        SHOOT_STATE.setNext(nextState);
        return SHOOT_STATE;
        
    } // end getChainedInstance
    
} // end ShootState
